#include <iostream>
#include"bezier_prediction/bezier_predict.h"
#include <ros/ros.h>
#include <pcl/point_types.h>
using namespace std;

extern "C"{
    void ndt_pythonbezier_predict_python(const float (*past_2d)[2], float (*pred_2d)[2]);
}


void ndt_pythonbezier_predict_python(const float (*past_2d)[2], float (*pred_2d)[2]) {
  Bezierpredict tgpredict;

  vector<Eigen::Vector4d> target_detect_list;
  for(int i = 0; i<_MAX_SEG;i++){
    target_detect_list.emplace_back(Eigen::Vector4d(past_2d[i][0], past_2d[i][1], 1, i*0.1));
  }

  int bezier_flag = tgpredict.TrackingGeneration(5,5,target_detect_list);
  // std::vector<Eigen::Matrix<double,6,1>> predict_state_list;
  std::vector<Eigen::Vector3d> Sample_list;
  if(bezier_flag==0){
      // predict_state_list = tgpredict.getStateListFromBezier(_PREDICT_SEG);
      Sample_list = tgpredict.SamplePoslist_bezier(_PREDICT_SEG);
      for(int i = 0; i<_PREDICT_SEG -1;i++){
        pred_2d[i][0] = Sample_list[i+1](0);
        pred_2d[i][1] = Sample_list[i+1](1);
      }
  }else{
      for(int i = 0; i<_PREDICT_SEG -1;i++){
        pred_2d[i][0] = 0;
        pred_2d[i][1] = 0; 
      }
  }

}
